#!/usr/bin/env python3
# -*- coding: utf-8 -*-

# Copyright 2019 Wuhan PS-Micro Technology Co., Itd.
#
# 基于Apache License 2.0许可协议
# 除非符合协议规定或书面同意，否则本软件按"原样"分发
# 详情参见许可协议：http://www.apache.org/licenses/LICENSE-2.0

import rospy, sys
import moveit_commander  # MoveIt!的Python接口库
import traceback
from moveit_commander import exception as moveit_exception
from moveit_commander import MoveGroupCommander  # 运动规划组控制类
from geometry_msgs.msg import Pose  # 位姿消息类型
from copy import deepcopy  # 用于深拷贝对象

class MoveItCartesianDemo:
    def __init__(self):
        # 初始化move_group的API，必须在使用任何MoveIt!功能前调用
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点，节点名为'moveit_cartesian_demo'
        rospy.init_node('moveit_cartesian_demo', anonymous=True)

        # 从参数服务器获取是否使用笛卡尔空间规划的参数，默认为True
        # 笛卡尔空间规划会生成平滑的直线运动路径
        cartesian = rospy.get_param('~cartesian', True)
                      
        # 初始化机械臂控制组，'robot_arm'是在setup_assistant中配置的规划组名称
        arm = MoveGroupCommander('robot_arm')
        
        # 允许运动规划失败后重新规划，提高成功率
        arm.allow_replanning(True)
        
        # 设置目标位置所使用的参考坐标系，'base_link'是机器人基坐标系
        arm.set_pose_reference_frame('base_link')
                
        # 设置位置和姿态的允许误差
        # 位置误差：0.001米（1毫米）
        # 姿态误差：0.001弧度（约0.057度）
        arm.set_goal_position_tolerance(0.001)
        arm.set_goal_orientation_tolerance(0.001)
        
        # 设置最大加速度和速度的缩放因子（0.5表示50%的最大限制）
        # 降低速度和加速度可以使运动更平稳，适合调试
        arm.set_max_acceleration_scaling_factor(0.5)
        arm.set_max_velocity_scaling_factor(0.5)
        
        # 获取末端执行器（工具）的link名称
        end_effector_link = arm.get_end_effector_link()

        # 控制机械臂先回到初始化位置，'reset'是在setup_assistant中定义的命名姿态
        arm.set_named_target('reset')
        arm.go()  # 执行运动指令
        rospy.sleep(1)  # 等待1秒，确保运动完成
                                               
        # 获取当前末端执行器的位姿，作为运动的起始位姿
        start_pose = arm.get_current_pose(end_effector_link).pose
                
        # 初始化路点列表，用于存储笛卡尔路径的目标点
        waypoints = []

        # 如果使用笛卡尔规划，将初始位姿加入路点列表
        if cartesian:
            waypoints.append(start_pose)
            
        # 定义第一个目标点：在起始位置基础上z轴方向向下移动0.1米
        wpose = deepcopy(start_pose)  # 深拷贝，避免修改原始起始位姿
        wpose.position.z -= 0.1  # z轴负方向移动（向下）

        # 根据是否使用笛卡尔规划，添加路点或直接运动
        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)  # 设置目标位姿
            arm.go()  # 执行运动
            rospy.sleep(1)  # 等待运动完成

        # 定义第二个目标点：在第一个点基础上x轴方向移动0.05米
        wpose.position.x += 0.1

        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)
        
        # 定义第三个目标点：在第二个点基础上y轴方向移动0.05米
        wpose.position.y += 0.1

        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)

        # 定义第四个目标点：回到起始位置的正下方（完成矩形路径）
        wpose.position.x -= 0.1
        wpose.position.y -= 0.1

        if cartesian:
            waypoints.append(deepcopy(wpose))
        else:
            arm.set_pose_target(wpose)
            arm.go()
            rospy.sleep(1)

        # 如果使用笛卡尔空间规划
        if cartesian:
            fraction = 0.0   # 路径规划覆盖率（0.0表示0%，1.0表示100%）
            maxtries = 100   # 最大尝试规划次数
            attempts = 0     # 已经尝试的次数
            
            # 设置机械臂当前的状态作为运动初始状态
            arm.set_start_state_to_current_state()
        
            # 尝试规划一条笛卡尔空间路径，依次通过所有路点
            # 参数说明：
            # waypoints: 路点列表
            # 0.01: 路径点之间的步长（米）
            # True: 忽略姿态的变化（保持末端执行器姿态不变）
            while fraction < 1.0 and attempts < maxtries:
                (plan, fraction) = arm.compute_cartesian_path(waypoints, 0.01, True)

                # 尝试次数累加
                attempts += 1
                
                # 每10次尝试打印一次信息
                if attempts % 10 == 0:
                    rospy.loginfo("Still trying after " + str(attempts) + " attempts...")
                    
            # 如果路径规划成功（覆盖率100%），执行路径
            if fraction == 1.0:
                rospy.loginfo("Path computed successfully. Moving the arm.")
                arm.execute(plan)  # 执行规划好的路径
                rospy.loginfo("Path execution complete.")
            # 如果路径规划失败，打印失败信息
            else:
                rospy.loginfo("Path planning failed with only " + str(fraction) + 
                              " success after " + str(maxtries) + " attempts.")  

            rospy.sleep(1)  # 等待运动完成


        # 运动结束后，控制机械臂回到初始化位置
        arm.set_named_target('reset')
        arm.go()
        rospy.sleep(1)
        
        # 关闭并退出moveit
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)


if __name__ == "__main__":
    try:
        # 创建并运行演示实例
        MoveItCartesianDemo()
    except rospy.ROSInterruptException:
        # 捕获ROS中断异常
        pass
